from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess

# Retrieving path information
from ament_index_python import get_package_share_directory
from pathlib import Path

# Utilizing launch files from other packages
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

# Working wirh environment variables
from launch.actions import SetEnvironmentVariable

# Simulation event handling
from launch.actions import RegisterEventHandler, EmitEvent
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown

# Path Variables
ignition_ros_package_path = get_package_share_directory("ros_gz_sim")
ros2_py_pkg_path = get_package_share_directory("ros2_py_pkg")
simulation_worlds_file_path = Path(ros2_py_pkg_path, "worlds/camera_simulation_project.world").as_posix()

# Can't use with event handlers 
# simulation = IncludeLaunchDescription(
#                 PythonLaunchDescriptionSource(
#                     launch_file_path=Path(ignition_ros_package_path, "launch/gz_sim.launch.py").as_posix()
#                 ),
#                 launch_arguments={"gz_args": "-r " + simulation_worlds_file_path}.items()
#             )

simulation = ExecuteProcess(
                cmd = ["ign", "gazebo", "-r", simulation_worlds_file_path],
                output = "screen"
             )

def generate_launch_description():
    return LaunchDescription([
        # Run ignition gazebo 

        # method 1
        # ExecuteProcess(
        #     cmd = ["ign", "gazebo", "worlds/camera_simulation_test.world"],
        #     additional_env = {"IGN_GAZEBO_RESOURCE_PATH": "/home/covirtue/ROS2-Navigation/src/ros2_py_pkg"},
        #     output = "screen"
        # ),

        # method 2
        SetEnvironmentVariable(
            name="IGN_GAZEBO_RESOURCE_PATH",
            value=ros2_py_pkg_path
        ),
        simulation,

        # Remapping topic
        Node(
            package="ros_gz_bridge",
            executable="parameter_bridge",
            arguments=[
                "/cmd_vel@geometry_msgs/msg/Twist@ignition.msgs.Twist",
                "/camera@sensor_msgs/msg/Image@ignition.msgs.Image",
                "/rods_camera_joint_cmd@std_msgs/msg/Float64@ignition.msgs.Double"
            ],
            # ros_arguments=["-r /model/box_wheel/cmd_vel:=/cmd_vel"], # Remapping topic in terminal
            remappings=[
                # ("/model/camera_robot/cmd_vel", "/cmd_vel"),
                ("/camera", "/camera/raw_image"),
                # ("/rods_camera_joint_cmd", "/rods_camera_joint_cmd")
            ],
            output="screen"

        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=simulation,
                on_exit=[EmitEvent(event=Shutdown())],
            )
        )
    ])